#include <cstdio>

#include "camera_info_manager/camera_info_manager.hpp"
#include "image_transport/image_transport.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/utilities.hpp"
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <cv_bridge/cv_bridge.h>

namespace binocular_driver
{
    class binocular_driver_node : public rclcpp::Node
    {
    public:
        explicit binocular_driver_node(const rclcpp::NodeOptions &options)
            : Node("binocular_driver", options)
        {
            RCLCPP_INFO(this->get_logger(), "Starting binocular_driver Node!");

            init_params();
            init_camera_info();

            r_cam_pub_ = image_transport::create_camera_publisher(this, r_cam_topic_, qos_for_sending);
            l_cam_pub_ = image_transport::create_camera_publisher(this, l_cam_topic_, qos_for_sending);

            // raw_cam_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
            //     raw_cam_topic_, rclcpp::SensorDataQoS(),
            //     std::bind(&binocular_driver_node::capture_callback, this, std::placeholders::_1));
            // 使用10个线程的线程池处理回调
            auto callback_group = this->create_callback_group(
                rclcpp::CallbackGroupType::MutuallyExclusive);
            
            auto sub_opt = rclcpp::SubscriptionOptions();
            sub_opt.callback_group = callback_group;

            raw_cam_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
                raw_cam_topic_, rclcpp::SensorDataQoS(),
                std::bind(&binocular_driver_node::capture_callback, this, std::placeholders::_1),
                sub_opt);
        }

    private:
        void init_params()
        {
            use_sensor_data_qos = this->declare_parameter("use_sensor_data_qos", true);
            qos_for_sending = use_sensor_data_qos ? rmw_qos_profile_sensor_data : rmw_qos_profile_default;

            cutting_posi_ = this->declare_parameter("cutting_posi", 200);
            raw_cam_topic_ = this->declare_parameter<std::string>("raw_camera_topic", "/usb_cam_raw/image_raw");
            r_cam_topic_ = this->declare_parameter<std::string>("right_camera_topic", "/camera/right/image_raw");
            l_cam_topic_ = this->declare_parameter<std::string>("left_camera_topic", "/camera/left/image_raw");

            camera_name_ = this->declare_parameter<std::string>("camera_name", "bino_camera");
            r_frame_id_ = this->declare_parameter<std::string>("r_frame_id", "r_camera_optical_frame");
            l_frame_id_ = this->declare_parameter<std::string>("l_frame_id", "l_camera_optical_frame");

            r_cam_info_url = this->declare_parameter<std::string>(
                "r_camera_info_url", "package://binocular_driver/config/r_camera_info.yaml");
            l_cam_info_url = this->declare_parameter<std::string>(
                "l_camera_info_url", "package://binocular_driver/config/l_camera_info.yaml");
        }

        void init_camera_info()
        {
            camera_info_manager_ =
                std::make_unique<camera_info_manager::CameraInfoManager>(this, camera_name_);

            //init right camera info
            if (camera_info_manager_->validateURL(r_cam_info_url))
            {
                camera_info_manager_->loadCameraInfo(r_cam_info_url);
                r_camera_info_msg_ = camera_info_manager_->getCameraInfo();
            }
            else
            {
                RCLCPP_WARN(this->get_logger(), "Invalid r_camera info URL: %s", r_cam_info_url.c_str());
            }
            //init left camera info
            if (camera_info_manager_->validateURL(l_cam_info_url))
            {
                camera_info_manager_->loadCameraInfo(l_cam_info_url);
                l_camera_info_msg_ = camera_info_manager_->getCameraInfo();
            }
            else
            {
                RCLCPP_WARN(this->get_logger(), "Invalid l_camera info URL: %s", l_cam_info_url.c_str());
            }
        }
        void capture_callback(const sensor_msgs::msg::Image::ConstSharedPtr &msg)
        {
            if (msg->data.size() < cutting_posi_)
            {
                RCLCPP_WARN(this->get_logger(), "Image data size is smaller than cutting position, skipping.");
                return;
            }

            const cv::Mat_<cv::Vec3b> ori_img = cv_bridge::toCvShare(msg, "rgb8")->image;
            if (ori_img.empty())
            {
                RCLCPP_WARN(this->get_logger(), "Received empty image, skipping.");
                return;
            }

            const auto now_stamp = this->now();

            // Cut the image into two halves
            cv::Mat l_image = ori_img(cv::Rect(0, 0, cutting_posi_, ori_img.rows));
            cv::Mat r_image = ori_img(cv::Rect(cutting_posi_, 0, ori_img.cols - cutting_posi_, ori_img.rows));

            auto r_camera_info = std::make_unique<sensor_msgs::msg::CameraInfo>(r_camera_info_msg_);
            auto l_camera_info = std::make_unique<sensor_msgs::msg::CameraInfo>(l_camera_info_msg_);
            
            // 设置消息头
            r_camera_info->header.stamp = now_stamp;
            r_camera_info->header.frame_id = r_frame_id_;
            l_camera_info->header.stamp = now_stamp;
            l_camera_info->header.frame_id = l_frame_id_;

            // 直接创建图像消息避免额外拷贝
            auto r_img_msg = cv_bridge::CvImage(r_camera_info->header, "rgb8", r_image).toImageMsg();
            auto l_img_msg = cv_bridge::CvImage(l_camera_info->header, "rgb8", l_image).toImageMsg();

            // 异步发布
            r_cam_pub_.publish(*r_img_msg, *r_camera_info);
            l_cam_pub_.publish(*l_img_msg, *l_camera_info);

        }

        uint32_t cutting_posi_;
        std::string raw_cam_topic_;
        std::string r_cam_topic_;
        std::string l_cam_topic_;
        std::string camera_name_;
        std::string r_frame_id_;
        std::string l_frame_id_;
        std::string r_cam_info_url;
        std::string l_cam_info_url;
        image_transport::CameraPublisher r_cam_pub_;
        image_transport::CameraPublisher l_cam_pub_;
        bool use_sensor_data_qos;
        rmw_qos_profile_t qos_for_sending;

        std::unique_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;

        rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr raw_cam_sub_;
        sensor_msgs::msg::CameraInfo r_camera_info_msg_;
        sensor_msgs::msg::CameraInfo l_camera_info_msg_;
    };

} // namespace binocular_driver

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(binocular_driver::binocular_driver_node)
